// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DHParameterTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Kinematics
{
    using System;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test DHParameter class
    /// </summary>
    [TestClass]
    public class DHParameterTests
    {
        /// <summary>
        /// Test DH parameter
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsTestDHParameter()
        {
            double alpha = 1;
            double a = 2;
            double theta = -1;
            double d = 4;

            DHParameter dh1 = DHParameter.CreateDHParameterForRevoluteJoint(alpha, a, d);
            Assert.AreEqual(alpha, dh1.ParameterAlpha);
            Assert.AreEqual(a, dh1.ParameterA);
            Assert.AreEqual(double.NaN, dh1.ParameterTheta);
            Assert.AreEqual(d, dh1.ParameterD);

            DHParameter dh2 = DHParameter.CreateDHParameterForPrismaticJoint(alpha, a, theta);
            Assert.AreEqual(alpha, dh2.ParameterAlpha);
            Assert.AreEqual(a, dh2.ParameterA);
            Assert.AreEqual(theta, dh2.ParameterTheta);
            Assert.AreEqual(double.NaN, dh2.ParameterD);
        }

        /// <summary>
        /// Test transformation matrix computation from DH parameters
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsTestDHParameterTransform()
        {
            double alpha = MathConstants.PIOverTwo;
            double a = 0;
            double theta = 1;
            double d = 3;
            double value = 10;

            DHParameter dhPrismatic = DHParameter.CreateDHParameterForPrismaticJoint(alpha, a, theta);
            DHParameter dhRevolute = DHParameter.CreateDHParameterForRevoluteJoint(alpha, a, d);

            Matrix4 rotx = HomogenousTransform.CreateFromRotationX(alpha);
            Matrix4 translx = HomogenousTransform.CreateFromTranslation(Vector3.XAxis * a);

            // This is a prismatic joint
            Matrix4 rotz = HomogenousTransform.CreateFromRotationZ(theta);
            Matrix4 translz = HomogenousTransform.CreateFromTranslation(Vector3.ZAxis * value);

            PoseDiff diff = new PoseDiff(dhPrismatic.GetTransformMatrix(value), rotx * translx * rotz * translz);
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3);

            // This is a revolute joint
            rotz = HomogenousTransform.CreateFromRotationZ(value);
            translz = HomogenousTransform.CreateFromTranslation(Vector3.ZAxis * d);

            diff = new PoseDiff(dhRevolute.GetTransformMatrix(value), rotx * translx * rotz * translz);
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3);
        }
    }
}
